using System;
using System.Collections.Generic;
using System.Text;
using LocalizationTest.Tools;
using System.Drawing;

namespace LocalizationTest
{
  class DoorSensor : Sensor
  {
    public override Measurement PredictForSimulation(WorldState worldState, Map map)
    {
      if (Globals.selfLocatorProperties.SensorNoise == NoiseType.noNoise)
        return PredictPerfect(worldState, map);
      else
      {
        Measurement perfectMeasurement = PredictPerfect(worldState, map);
        Measurement noisyMeasurementTrue = new DetectorMeasurement(true, this);
        Measurement noisyMeasurementFalse = new DetectorMeasurement(false, this);
        double pDetectDoor = GetMeasurementProbability(noisyMeasurementTrue, perfectMeasurement);
        double pNotDetectDoor = GetMeasurementProbability(noisyMeasurementFalse, perfectMeasurement);

        Random random = new Random();
        double rnd = random.NextDouble();

        if (rnd < pDetectDoor) return noisyMeasurementTrue;
        else return noisyMeasurementFalse;
      }
    }

    public override Measurement PredictPerfect(WorldState worldState, Map map)
    {
      int index = Globals.PositionToIndex((worldState as WorldState1D).Position, (map as MapWithDoors).RasterWidth, Globals.stateSpaceMinimumX);
      if (index < 0 || index >= (map as MapWithDoors).DoorsAsArray.Length) return new DetectorMeasurement(false, this);
      bool prediction = (map as MapWithDoors).DoorsAsArray[index];
      return new DetectorMeasurement(prediction, this);
    }

    private double GetMeasurementProbability(Measurement noisyMeasurement, Measurement perfectMeasurement)
    {
      double probability = 0.0;
      bool noisy = (noisyMeasurement as DetectorMeasurement).SomethingWasDetected;
      bool perfect = (perfectMeasurement as DetectorMeasurement).SomethingWasDetected;

      if (noisy == true && perfect == true) probability = 0.95;
      if (noisy == false && perfect == true) probability = 0.05;
      if (noisy == true && perfect == false) probability = 0.3;
      if (noisy == false && perfect == false) probability = 0.7;
      return probability;
    }

    public override double GetMeasurementProbability(WorldState worldState, Map map, Measurement measurement)
    {
      Measurement perfectMeasurement = PredictPerfect(worldState, map);
      Measurement noisyMeasurement = measurement;
      double probability = GetMeasurementProbability(noisyMeasurement, perfectMeasurement);
      return probability;
    }

    public override void Draw(Measurement measurement, WorldState worldState, double shade, string drawingID)
    {
      if (worldState is WorldState1D)
      {
        double position = (worldState as WorldState1D).Position;
        Color color;
        if (shade == 0.9999) color = Color.Gray;
        else
        {
          if ((measurement as DetectorMeasurement).SomethingWasDetected)
            color = Color.FromArgb((int)(255.0), (int)(200.0 * shade), (int)(200.0 * shade));
          else
            color = Color.FromArgb((int)(200.0 * shade), (int)(255.0), (int)(200.0 * shade));
        }
        DebugDrawingTool.CreateLine(drawingID, new Vector2(position, 0), new Vector2(position, -40), Globals.rasterWidth - 2, color);

      }
    }

    public override string ToString()
    {
      throw new Exception("The method or operation is not implemented.");
    }
  }
}
